#include "ROBOT.h"

void ROBOT::DriveVelocity(float vcm, float vdf)
{
	float rv,lv;
	float dis_rate;
	float over_vel = 30;

	rv = (vcm + vdf)/2;
	lv = (vcm - vdf)/2;
	
	if(lv>=over_vel)
	{
		dis_rate = over_vel/lv;
		lv = over_vel;
		rv = rvel*dis_rate;
	}
	if(rv>=over_vel)
	{
		dis_rate = over_vel/rv;
		rv = over_vel;
		lv = lv*dis_rate;
	}

	Vcm_ = rv + lv;
	Vdf_ = rv - lv;

}

void ROBOT::DriveVelocity2(float rv, float lv)
{
	float dis_rate;
	float over_vel = 30;

	if(lv>=over_vel)
	{
		dis_rate = over_vel/lv;
		lv = over_vel;
		rv = rvel*dis_rate;
	}
	if(rv>=over_vel)
	{
		dis_rate = over_vel/rv;
		rv = over_vel;
		lv = lv*dis_rate;
	}

	Vcm_ = (rv+lv)/2;
	Vdf_ = (rv-lv)/2;

}

void ROBOT::DriveMotors(float lv, float rv)
{
	float dis_rate;
	float over_vel = 30;

	if(lv>=over_vel)
	{
		dis_rate = over_vel/lv;
		lv = over_vel;
		rv = rvel*dis_rate;
	}
	if(rv>=over_vel)
	{
		dis_rate = over_vel/rv;
		rv = over_vel;
		lv = lv*dis_rate;
	}

	Vcm_ = (rv+lv)/2;
	Vdf_ = (rv-lv)/2;

}


void ROBOT::DriveStop()
{
	Vcm_ = 0;
	Vdf_ = 0;
}

void ROBOT::DriveFastStop()
{
	Vcm_ = 0;
	Vdf_ = 0;
}


void ROBOT::SuctionMotor(int type)
{

}
void ROBOT::SideBlower(int type)
{
	
}
void ROBOT::SideBlowerOn(void)
{

}

void ROBOT::SideBlowerOff(void)
{

}

void ROBOT::ControlVoice(unsigned char val)
{

}

